#include "geometry_msgs/msg/twist.hpp"
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <rosbag2_cpp/reader.hpp>

using geometry_msgs::msg::Twist;
class bagReader : public rclcpp::Node {

  // 读取rosbag2的内容
  std::unique_ptr<rosbag2_cpp::Reader> reader_;

  // 发布读取的内容
  rclcpp::Publisher<Twist>::SharedPtr pub_;

public:
  bagReader() : rclcpp::Node("bag_Reader") {
    RCLCPP_INFO(this->get_logger(), "启动读取rosbag2内容节点");

    reader_ = std::make_unique<rosbag2_cpp::Reader>();
    reader_->open("my_bag");

    pub_ = this->create_publisher<Twist>("turtle1/pose", 10);

    // 读取rosbag2的内容并发布
    while (reader_->has_next()) {
      auto pose_msg = reader_->read_next<Twist>();

      pub_->publish(pose_msg);
      RCLCPP_INFO(this->get_logger(), "线速度: x=%f, y=%f, 角速度=%f",
                  pose_msg.linear.x, pose_msg.linear.y, pose_msg.angular.z);
    }

    reader_->close();
    RCLCPP_INFO(this->get_logger(),
                "Finished reading and publishing all messages from rosbag2.");
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<bagReader>());
  rclcpp::shutdown();
  return 0;
}
